#region License

// Copyright 2007-2008 Rory Plaire (codekaizen@gmail.com)
/*
 * This license governs use of the accompanying software. If you use the software, you
 * accept this license. If you do not accept the license, do not use the software.
 * 
 * 1. Definitions
 * The terms "reproduce," "reproduction," "derivative works," and "distribution" have the
 * same meaning here as under U.S. copyright law.
 * A "contribution" is the original software, or any additions or changes to the software.
 * A "contributor" is any person that distributes its contribution under this license.
 * "Licensed patents" are a contributor's patent claims that read directly on its contribution.
 * 
 * 2. Grant of Rights
 * (A) Copyright Grant- Subject to the terms of this license, including the license conditions 
 *     and limitations in section 3, each contributor grants you a non-exclusive, worldwide, 
 *     royalty-free copyright license to reproduce its contribution, prepare derivative works 
 *     of its contribution, and distribute its contribution or any derivative works that you create.
 * (B) Patent Grant- Subject to the terms of this license, including the license conditions and 
 *     limitations in section 3, each contributor grants you a non-exclusive, worldwide, royalty-free 
 *     license under its licensed patents to make, have made, use, sell, offer for sale, import, 
 *     and/or otherwise dispose of its contribution in the software or derivative works of the 
 *     contribution in the software.
 * 
 * 3. Conditions and Limitations
 * (A) No Trademark License- This license does not grant you rights to use any contributors' 
 *     name, logo, or trademarks.
 * (B) If you bring a patent claim against any contributor over patents that you claim are 
 *     infringed by the software, your patent license from such contributor to the software 
 *     ends automatically.
 * (C) If you distribute any portion of the software, you must retain all copyright, patent, 
 *     trademark, and attribution notices that are present in the software.
 * (D) If you distribute any portion of the software in source code form, you may do so only under 
 *     this license by including a complete copy of this license with your distribution. If you 
 *     distribute any portion of the software in compiled or object code form, you may only do so 
 *     under a license that complies with this license.
 * (E) The software is licensed "as-is." You bear the risk of using it. The contributors give no 
 *     express warranties, guarantees or conditions. You may have additional consumer rights under 
 *     your local laws which this license cannot change. To the extent permitted under your local laws, 
 *     the contributors exclude the implied warranties of merchantability, fitness for a particular 
 *     purpose and non-infringement.
 * 
 */

#endregion

using System;
using System.Collections.Generic;
using NPack.Numeric;
using Xunit;
using IMatrixD = NPack.Numeric.Interfaces.IMatrix<System.Double>;
using IAffineD = NPack.Numeric.Interfaces.IAffineTransformMatrix<System.Double>;
using IVectorD = NPack.Numeric.Interfaces.IVector<System.Double>;
using MatrixD = NPack.Matrix.Matrix<System.Double>;
using AffineD = NPack.Matrix.AffineMatrix<System.Double>;
using VectorD = NPack.Matrix.Vector<System.Double>;

namespace NPack.Tests
{
    public class AffineMatrixTests
    {
        private static readonly IComparer<Double> _epsilonComparer = new EpsilonComparer();

        [Fact]
        public void AffineMatrixFromElementsMustBeSquare()
        {
            Double[][] elements = TestHelper.GenerateElements<Double>(4, 3, true);
            Assert.Throws<ArgumentException>(delegate { new AffineD(MatrixFormat.RowMajor, elements); });
        }

        [Fact]
        public void InvertTest()
        {
            AffineD identity = new AffineD(MatrixFormat.RowMajor, 4);
            AffineD a1 = new AffineD(MatrixFormat.RowMajor, new Double[][]
                                                                {
                                                                    new Double[] {5, 3, 2, 0},
                                                                    new Double[] {2, 3, 4, 0},
                                                                    new Double[] {10, 2, 3, 0},
                                                                    new Double[] {0, 0, 0, 1}
                                                                });
            AffineD inva1 = a1.Inverse;
            MatrixD product = a1.Multiply(inva1);

            for (Int32 row = 0; row < 4; row++)
            {
                for (Int32 column = 0; column < 4; column++)
                {
                    Assert.Equal((Double) identity[row, column], (Double) product[row, column], _epsilonComparer);
                }
            }
        }

        [Fact]
        public void ScaleTest()
        {
            AffineD m1 = new AffineD(MatrixFormat.RowMajor, 3);
            m1.Scale(5);

            Assert.Equal(5, (Double) m1[0, 0], _epsilonComparer);
            Assert.Equal(0, (Double) m1[0, 1], _epsilonComparer);
            Assert.Equal(0, (Double) m1[0, 2], _epsilonComparer);
            Assert.Equal(0, (Double) m1[1, 0], _epsilonComparer);
            Assert.Equal(5, (Double) m1[1, 1], _epsilonComparer);
            Assert.Equal(0, (Double) m1[1, 2], _epsilonComparer);
            Assert.Equal(0, (Double) m1[2, 0], _epsilonComparer);
            Assert.Equal(0, (Double) m1[2, 1], _epsilonComparer);
            Assert.Equal(1, (Double) m1[2, 2], _epsilonComparer);

            AffineD m2 = new AffineD(MatrixFormat.RowMajor, 4);
            m2.Scale(-0.4);

            Assert.Equal(-0.4, (Double) m2[0, 0], _epsilonComparer);
            Assert.Equal(0, (Double) m2[0, 1], _epsilonComparer);
            Assert.Equal(0, (Double) m2[0, 2], _epsilonComparer);
            Assert.Equal(0, (Double) m2[0, 3], _epsilonComparer);
            Assert.Equal(0, (Double) m2[1, 0], _epsilonComparer);
            Assert.Equal(-0.4, (Double) m2[1, 1], _epsilonComparer);
            Assert.Equal(0, (Double) m2[1, 2], _epsilonComparer);
            Assert.Equal(0, (Double) m2[1, 3], _epsilonComparer);
            Assert.Equal(0, (Double) m2[2, 0], _epsilonComparer);
            Assert.Equal(0, (Double) m2[2, 1], _epsilonComparer);
            Assert.Equal(-0.4, (Double) m2[2, 2], _epsilonComparer);
            Assert.Equal(0, (Double) m2[2, 3], _epsilonComparer);
            Assert.Equal(0, (Double) m2[3, 0], _epsilonComparer);
            Assert.Equal(0, (Double) m2[3, 1], _epsilonComparer);
            Assert.Equal(0, (Double) m2[3, 2], _epsilonComparer);
            Assert.Equal(1, (Double) m2[3, 3], _epsilonComparer);
        }

        [Fact]
        public void ScaleVectorTest()
        {
            AffineD m1 = new AffineD(MatrixFormat.RowMajor, 3);
            VectorD v1 = new VectorD(1, 3);
            m1.Scale(v1);

            Assert.Equal(1, (Double) m1[0, 0], _epsilonComparer);
            Assert.Equal(0, (Double) m1[0, 1], _epsilonComparer);
            Assert.Equal(0, (Double) m1[0, 2], _epsilonComparer);
            Assert.Equal(0, (Double) m1[1, 0], _epsilonComparer);
            Assert.Equal(3, (Double) m1[1, 1], _epsilonComparer);
            Assert.Equal(0, (Double) m1[1, 2], _epsilonComparer);
            Assert.Equal(0, (Double) m1[2, 0], _epsilonComparer);
            Assert.Equal(0, (Double) m1[2, 1], _epsilonComparer);
            Assert.Equal(1, (Double) m1[2, 2], _epsilonComparer);

            AffineD m2 = new AffineD(MatrixFormat.RowMajor, 4);
            VectorD v2 = new VectorD(0, -0.3, 80);
            m2.Scale(v2);

            Assert.Equal(0, (Double) m2[0, 0], _epsilonComparer);
            Assert.Equal(0, (Double) m2[0, 1], _epsilonComparer);
            Assert.Equal(0, (Double) m2[0, 2], _epsilonComparer);
            Assert.Equal(0, (Double) m2[0, 3], _epsilonComparer);
            Assert.Equal(0, (Double) m2[1, 0], _epsilonComparer);
            Assert.Equal(-0.3, (Double) m2[1, 1], _epsilonComparer);
            Assert.Equal(0, (Double) m2[1, 2], _epsilonComparer);
            Assert.Equal(0, (Double) m2[1, 3], _epsilonComparer);
            Assert.Equal(0, (Double) m2[2, 0], _epsilonComparer);
            Assert.Equal(0, (Double) m2[2, 1], _epsilonComparer);
            Assert.Equal(80, (Double) m2[2, 2], _epsilonComparer);
            Assert.Equal(0, (Double) m2[2, 3], _epsilonComparer);
            Assert.Equal(0, (Double) m2[3, 0], _epsilonComparer);
            Assert.Equal(0, (Double) m2[3, 1], _epsilonComparer);
            Assert.Equal(0, (Double) m2[3, 2], _epsilonComparer);
            Assert.Equal(1, (Double) m2[3, 3], _epsilonComparer);
        }

        [Fact]
        public void ScaleVectorMustNotHaveMoreComponentsAsLinearDimensionTest()
        {
            AffineD m1 = new AffineD(MatrixFormat.RowMajor, 3);
            VectorD v1 = new VectorD(1, 3, 2, 4);
            Assert.Throws<ArgumentException>(delegate { m1.Scale(v1); });
        }

        [Fact]
        public void RotateAlongTest()
        {
            // Test 2D rotation

            AffineD m1 = new AffineD(MatrixFormat.ColumnMajor, 3);
            VectorD v1 = new VectorD(1, 0, 1);
            Double angle = Math.PI/4;

            m1.RotateAlong(v1, angle);

            Assert.Equal(Math.Cos(angle), (Double) m1[0, 0], _epsilonComparer);
            Assert.Equal(-Math.Sin(angle), (Double) m1[0, 1], _epsilonComparer);
            Assert.Equal(0, (Double) m1[0, 2], _epsilonComparer);
            Assert.Equal(Math.Sin(angle), (Double) m1[1, 0], _epsilonComparer);
            Assert.Equal(Math.Cos(angle), (Double) m1[1, 1], _epsilonComparer);
            Assert.Equal(0, (Double) m1[1, 2], _epsilonComparer);
            Assert.Equal(0, (Double) m1[2, 0], _epsilonComparer);
            Assert.Equal(0, (Double) m1[2, 1], _epsilonComparer);
            Assert.Equal(1, (Double) m1[2, 2], _epsilonComparer);

            // Test 3D rotation

            AffineD m2 = new AffineD(MatrixFormat.ColumnMajor, 4);
            VectorD xAxis = new VectorD(1, 0, 0);
            VectorD yAxis = new VectorD(0, 1, 0);
            VectorD zAxis = new VectorD(0, 0, 1);

            Double[,] expected;

            m2.RotateAlong(xAxis, angle);
            expected = compute3dRotationCheck(angle, xAxis);

            for (Int32 i = 0; i < 4; i++)
            {
                for (Int32 j = 0; j < 4; j++)
                {
                    Assert.Equal(expected[i, j], (Double) m2[i, j], _epsilonComparer);
                }
            }

            m2.Reset();

            m2.RotateAlong(yAxis, angle);
            expected = compute3dRotationCheck(angle, yAxis);

            for (Int32 i = 0; i < 4; i++)
            {
                for (Int32 j = 0; j < 4; j++)
                {
                    Assert.Equal(expected[i, j], (Double) m2[i, j], _epsilonComparer);
                }
            }

            m2.Reset();

            m2.RotateAlong(zAxis, angle);
            expected = compute3dRotationCheck(angle, zAxis);

            for (Int32 i = 0; i < 4; i++)
            {
                for (Int32 j = 0; j < 4; j++)
                {
                    Assert.Equal(expected[i, j], (Double) m2[i, j], _epsilonComparer);
                }
            }
        }

        [Fact]
        public void RotateAlongVectorMustHaveSameNumberOfComponentsAsTransformDimensionTest()
        {
            AffineD m1 = new AffineD(MatrixFormat.RowMajor, 4);
            VectorD v1 = new VectorD(1, 0);
            Assert.Throws<ArgumentException>(delegate { m1.RotateAlong(v1, Math.PI/4); });
        }

        [Fact]
        public void RotateAtTest()
        {
            AffineD m1 = new AffineD(MatrixFormat.RowMajor, 3);
            VectorD rotateAtPoint = new VectorD(10, 0);

            m1.RotateAt(rotateAtPoint, null, Math.PI);

            VectorD expected = new VectorD(20, 0, 1);
            VectorD p1 = new VectorD(0, 0, 1);

            VectorD actual = m1.TransformVector(p1);

            Assert.NotSame(expected, actual);
            Assert.Equal(expected.ComponentCount, actual.ComponentCount);
            Assert.Equal((Double) expected[0], (Double) actual[0], _epsilonComparer);
            Assert.Equal((Double) expected[1], (Double) actual[1], _epsilonComparer);
        }

        [Fact]
        public void ShearTest()
        {
        }

        [Fact]
        public void TranslateTest()
        {
            AffineD m1 = new AffineD(MatrixFormat.RowMajor, 3);
            m1.Translate(new Real<double>(5));

            Assert.Equal(5, (Double) m1[2, 0], _epsilonComparer);
            Assert.Equal(5, (Double) m1[2, 1], _epsilonComparer);

            m1.Reset();

            VectorD v1 = new VectorD(-4.3, 0);
            m1.Translate(v1);

            Assert.Equal(-4.3, (Double) m1[2, 0], _epsilonComparer);
            Assert.Equal(0, (Double) m1[2, 1], _epsilonComparer);

            AffineD m2 = new AffineD(MatrixFormat.ColumnMajor, 4);
            m2.Translate(new Real<double>(5));

            Assert.Equal(5, (Double) m2[0, 3], _epsilonComparer);
            Assert.Equal(5, (Double) m2[1, 3], _epsilonComparer);
            Assert.Equal(5, (Double) m2[2, 3], _epsilonComparer);

            VectorD v2 = new VectorD(-4.3, 0, 10);
            m2.Translate(v2);

            Assert.Equal(0.7, (Double) m2[0, 3], _epsilonComparer);
            Assert.Equal(5, (Double) m2[1, 3], _epsilonComparer);
            Assert.Equal(15, (Double) m2[2, 3], _epsilonComparer);
        }

        [Fact]
        public void TransformVectorTest()
        {
            AffineD m1 = new AffineD(MatrixFormat.RowMajor, 3);
            m1.RotateAlong(null, Math.PI/4);

            VectorD v1 = new VectorD(1, 0, 1);
            VectorD v2 = new VectorD(0, 1, 1);

            VectorD v1T = m1.TransformVector(v1);
            VectorD v2T = m1.TransformVector(v2);

            Assert.Equal(Math.Cos(Math.PI/4), (Double) v1T[0], _epsilonComparer);
            Assert.Equal(Math.Sin(Math.PI/4), (Double) v1T[1], _epsilonComparer);

            // .... more stuff goes here
        }

        private static Double[,] compute3dRotationCheck(Double angle, VectorD axis)
        {
            Double sinTheta = Math.Sin(angle);
            Double cosTheta = Math.Cos(angle);
            Double x = axis[0];
            Double y = axis[1];
            Double z = axis[2];
            Double x2 = Math.Pow(x, 2);
            Double y2 = Math.Pow(y, 2);
            Double z2 = Math.Pow(z, 2);

            Double[,] rotationCheck = new Double[4,4];

            rotationCheck[0, 0] = cosTheta + (1 - cosTheta)*x2;
            rotationCheck[0, 1] = (1 - cosTheta)*x*y - sinTheta*z;
            rotationCheck[0, 2] = (1 - cosTheta)*x*z + sinTheta*y;
            rotationCheck[0, 3] = 0;

            rotationCheck[1, 0] = (1 - cosTheta)*x*y + sinTheta*z;
            rotationCheck[1, 1] = cosTheta + (1 - cosTheta)*y2;
            rotationCheck[1, 2] = (1 - cosTheta)*y*z - sinTheta*x;
            rotationCheck[1, 3] = 0;

            rotationCheck[2, 0] = (1 - cosTheta)*z*x - sinTheta*y;
            rotationCheck[2, 1] = (1 - cosTheta)*z*y + sinTheta*x;
            rotationCheck[2, 2] = cosTheta + (1 - cosTheta)*z2;
            rotationCheck[2, 3] = 0;

            rotationCheck[3, 0] = 0;
            rotationCheck[3, 1] = 0;
            rotationCheck[3, 2] = 0;
            rotationCheck[3, 3] = 1;

            return rotationCheck;
        }
    }
}